#! /usr/bin/env python

# Copyright (c) 2010, Arizona Robotics Research Group, University of Arizona
# All rights reserved.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# 
#     * Redistributions of source code must retain the above copyright
#       notice, this list of conditions and the following disclaimer.
#     * Redistributions in binary form must reproduce the above copyright
#       notice, this list of conditions and the following disclaimer in the
#       documentation and/or other materials provided with the distribution.
#     * Neither the name of the Willow Garage, Inc. nor the names of its
#       contributors may be used to endorse or promote products derived from
#       this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS'
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Matt Brockman
# Original Code: move_arm_demo.py & wubble_head_action.py by Ahn Tran

PKG = 'test_manipulation'
NAME = 'point_arm_service'

#copied from move_arm_demo
import roslib; roslib.load_manifest(PKG)
import rospy
import tf

from cs665_manipulation_srvs.srv import *
from actionlib import SimpleActionClient
from geometry_msgs.msg import *
from wubble_actions.msg import *
import math

class PointArmService():

    def __init__(self):
        rospy.init_node(NAME + 'server' , anonymous=True)
        self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
        self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
#        self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
#        self.move_client.wait_for_server()
        self.tf = tf.TransformListener()
        self.arm_client.wait_for_server()
        self.gripper_client.wait_for_server()

        self.service = rospy.Service('point_arm_service', ArmPointService, self.handle_point)
        rospy.loginfo("%s: Ready for pointing", NAME)

    #handles receiving a PointStamped.  Not handling preempts - does client do this?  remember to check code
    def handle_point(self, req):
        success = False
        #print info about request
        rospy.loginfo("%s: recieved position %s %s %s in %s", \
                        NAME, req.position.point.x, req.position.point.y, req.position.point.z, req.position.header.frame_id);
        if self.point_at(req.position.header.frame_id, req.position.point.x, req.position.point.y, req.position.point.z):
            success = True      
        return success


    #(almost) blatent copy / past from wubble_head_action.py.  Err, it's going to the wrong position
    def transform_target_point(self, point):

        # Cannot use arm_shoulder_pan_link & arm_shoulder_tilt_link as target frame_id
        # because the angles used by the low-level motors are absolute, and fixed to
        # the arm_base_link coordinate (not relative to the coordinates of pan & tilt)
        target_frame = 'arm_base_link'
        
        #rospy.loginfo("%s: got %s %s %s %s", NAME, point.header.frame_id, point.point.x, point.point.y, point.point.z)

        # Wait for tf info (time-out in 5 seconds)
        self.tf.waitForTransform(target_frame, point.header.frame_id, rospy.Time(), rospy.Duration(2.0))

        # Transform target point & retrieve the pan angle
        pan_target = self.tf.transformPoint(target_frame, point)
        pan_angle = math.atan2(pan_target.point.y, pan_target.point.x)
        #rospy.loginfo("%s: Pan transformed to <%s, %s, %s> => angle %s", \
        #        NAME, pan_target.point.x, pan_target.point.y, pan_target.point.z, pan_angle)

        # Transform target point & retrieve the tilt angle
        tilt_target = self.tf.transformPoint(target_frame, point)
        tilt_angle = math.atan2(tilt_target.point.z,
                math.sqrt(math.pow(tilt_target.point.x, 2) + math.pow(tilt_target.point.y, 2)))
        #rospy.loginfo("%s: Tilt transformed to <%s, %s, %s> => angle %s", \
        #        NAME, tilt_target.point.x, tilt_target.point.y, tilt_target.point.z, tilt_angle)

        return [pan_angle, tilt_angle]

    #point!  modified move_arm + reach_at from move_arm_demo
    def point_at(self, frame_id, x, y, z):

        goal = SmartArmGoal()
        goal.target_point = PointStamped()
        goal.target_point.header.frame_id = frame_id
        goal.target_point.point.x = x
        goal.target_point.point.y = y
        goal.target_point.point.z = z
        
        self.move_gripper (-2,2)


        target_shoulder = list()
        target_shoulder = self.transform_target_point(goal.target_point)

        if (target_shoulder[0] < 1.2 and target_shoulder[0] > -1.2):

            #bend arm a bit
            goal.target_joints = [target_shoulder[0], target_shoulder[1] + .2, .3, .5]

            # Sends the goal to the action server.
            self.arm_client.send_goal(goal)
            self.arm_client.wait_for_result(rospy.Duration(2.0))

            #straighten arm
            goal.target_joints = [target_shoulder[0], target_shoulder[1], 0, 0]

            # Sends the goal to the action server.
            self.arm_client.send_goal(goal)
            self.arm_client.wait_for_result()
            # Return result
            #something broke here, returning True fix is temporary
            return self.arm_client.get_result()

        #motors are too slow in the demo to actually wait for arm to go to cobra position before moving
        self.go_to_cobra()
        rospy.sleep(2)        
        position = Point(x, y, z)
        orientation = Quaternion(w=1.0)
#        self.move_to('/map', position, orientation, 99)

        return self.point_at(frame_id, x, y, z)

    #I stole this from somewhere
#        goal = ErraticBaseGoal()
#        goal.target_pose.header.stamp = rospy.Time.now()
#        goal.target_pose.header.frame_id = frame_id
#        goal.target_pose.pose.position = position
#        goal.target_pose.pose.orientation = orientation
#        goal.vicinity_range = vicinity

#        self.move_client.send_goal(goal)
#        self.move_client.wait_for_result()

    def go_to_cobra(self):
        goal = SmartArmGoal()
        goal.target_point = PointStamped()
        goal.target_point.header.frame_id = 'arm_base_link'
        goal.target_point.point.x = 0
        goal.target_point.point.y = 0
        goal.target_point.point.z = 0
        goal.target_joints = [0.0, 1.972222, -1.972222, 0.0]
        self.arm_client.send_goal(goal)
        self.arm_client.wait_for_result()
        return self.arm_client.get_result()

    #stolen gripper code from Anh Tran's "block_stacking_actions.py" in wubble_blocks
    #would be cool if could point with a single claw rather than both (or with 'thumb'?)
    def move_gripper(self, left_finger, right_finger):
        success = False        
        goal = SmartArmGripperGoal()
        goal.target_joints = [left_finger, right_finger]
        self.gripper_client.send_goal(goal)
        self.gripper_client.wait_for_result()
        if self.gripper_client.get_result():
            success = True
        return success

if __name__ == '__main__':
    try:
        pointle = PointArmService()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
